/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
 This software may be distributed and modified under the terms of the GNU
 General Public License version 2 (GPL2) as published by the Free Software
 Foundation and appearing in the file GPL2.TXT included in the packaging of
 this file. Please note that GPL2 Section 2[b] requires that all works based
 on this software must also be made publicly available under the terms of
 the GPL2 ("Copyleft").
 Contact information
 -------------------
 Kristian Lauszus, TKJ Electronics
 Web      :  http://www.tkjelectronics.com
 e-mail   :  kristianl@tkjelectronics.com
 */

#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

#include <Wire.h>

#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
#define CTRL_REG5 0x24

int L3G4200D_Address = 105; //I2C address of the L3G4200D

Kalman kalmanX;
Kalman kalmanY;
Kalman kalmanZ;

#define gX A0
#define gY A1
#define gZ A2

#define aX A0
#define aY A1
#define aZ A2

int x;
int y;
int z;

double zeroValue[6] = { 0 }; // gyroX, gyroY, gyroZ, accX, accY, accZ

/* All the angles start at 180 degrees */
double gyroXangle = 180;
double gyroYangle = 180;
double gyroZangle = 180;

// Complimentary filter
double compAngleX = 180;
double compAngleY = 180;
double compAngleZ = -180;

// Used for timing
unsigned long timer;

void setup() {
  analogReference(EXTERNAL); // 3.3V
  Serial.begin(9600);
  Serial1.begin(9600);
  delay(100);//wait for the sensor to get ready

  Wire.begin();
  Serial.println("starting up L3G4200D");
  setupL3G4200D(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec

  delay(500); //wait for the sensor to be ready 

  //getGyroValues();  // This will update x, y, and z with new values
  // Calibrate all sensors in horizontal position
  for (uint8_t i = 0; i < 100; i++) { // Take the average of 100 readings
    getGyroValues();  // This will update x, y, and z with new values
    zeroValue[0] += x;
    zeroValue[1] += y;
    zeroValue[2] += z;
    zeroValue[3] += analogRead(aX);
    zeroValue[4] += analogRead(aY);
    zeroValue[5] += analogRead(aZ);
    delay(10);
  }
  zeroValue[0] /= 100;
  zeroValue[1] /= 100;
  zeroValue[2] /= 100;
  zeroValue[3] /= 100;
  zeroValue[4] /= 100;
  zeroValue[5] /= 100;
  zeroValue[5] -= 102.3; // Z value is -1g when facing upwards - Sensitivity = 0.33/3.3*1023=102.3

  kalmanX.setAngle(180); // Set starting angle
  kalmanY.setAngle(180);
  kalmanZ.setAngle(-180);
  timer = micros(); // start timing
}

void loop() {
  getGyroValues();  // This will update x, y, and z with new values
  double gyroXrate = -((x - zeroValue[0]) / 1.0323); // (gyroXadc-gryoZeroX)/Sensitivity - in quids - Sensitivity = 0.00333/3.3*1023=1.0323
  gyroXangle += gyroXrate * ((double)(micros() - timer) / 1000000); // Without any filter

  double gyroYrate = -((y - zeroValue[1]) / 1.0323);
  gyroYangle += gyroYrate * ((double)(micros() - timer) / 1000000);

  
  double gyroZrate = -((z-zeroValue[2])/1.0323);
  gyroZangle += gyroZrate*((double)(micros()-timer)/1000000);
  //Serial.println(gyroZangle); // This is the yaw
  

  double accXval = (double)analogRead(aX) - zeroValue[3];
  double accYval = (double)analogRead(aY) - zeroValue[4];
  double accZval = (double)(analogRead(aZ) - 102.3) - zeroValue[5];

  // Convert to 360 degrees resolution
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We are then convert it to 0 to 2π and then from radians to degrees
  double accXangle = (atan2(accXval, accZval) + PI) * RAD_TO_DEG;
  double accYangle = (atan2(accYval, accZval) + PI) * RAD_TO_DEG;
  double accZangle = (atan2(accZval, accYval) + PI) * RAD_TO_DEG;


  /* You might have to tune the filters to get the best values */
  compAngleX = (0.98 * (compAngleX + (gyroXrate * (double)(micros() - timer) / 1000000))) + (0.02 * (accXangle));
  compAngleY = (0.98 * (compAngleY + (gyroYrate * (double)(micros() - timer) / 1000000))) + (0.02 * (accYangle));
  compAngleZ = (0.98 * (compAngleZ + (gyroZrate * (double)(micros() - timer) / 1000000))) + (0.02 * (accZangle));

  double xAngle = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros() - timer));
  double yAngle = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros() - timer));
  double zAngle = kalmanZ.getAngle(accZangle, gyroZrate, (double)(micros() - timer));

  timer = micros(); // reset timing

  /* print data to processing */
//  Serial.print(gyroXangle); Serial.print("\t");
//  Serial.print(gyroYangle); Serial.print("\t");
//
//  Serial.print(accXangle); Serial.print("\t");
//  Serial.print(accYangle); Serial.print("\t");
//
//  Serial.print(compAngleX); Serial.print("\t");
//  Serial.print(compAngleY); Serial.print("\t");

  int zA = (int)zAngle;
  int gyZ = (int)gyroZrate;

  Serial.print(xAngle); Serial.print("\t");
  Serial.print(yAngle); Serial.print("\t");
  Serial.print(zAngle); Serial.print("\t");
  Serial.print(zA); Serial.print("\t");

  Serial.print(gyroXrate); Serial.print("\t");
  Serial.print(gyroYrate); Serial.print("\t");
  Serial.print(gyroZrate); Serial.print("\t");
  Serial.print(gyZ); Serial.print("\t");

  Serial.print("\n");


  Serial1.write(zA >> 8 & 0xff);
  Serial1.write(zA & 0xff);
  Serial1.write(gyZ >> 8 & 0xff);
  Serial1.write(gyZ & 0xff);
  //Serial1.write(gyZ);
  
  delay(1000);
}


void getGyroValues(){

  byte xMSB = readRegister(L3G4200D_Address, 0x29);
  byte xLSB = readRegister(L3G4200D_Address, 0x28);
  x = ((xMSB << 8) | xLSB);

  byte yMSB = readRegister(L3G4200D_Address, 0x2B);
  byte yLSB = readRegister(L3G4200D_Address, 0x2A);
  y = ((yMSB << 8) | yLSB);

  byte zMSB = readRegister(L3G4200D_Address, 0x2D);
  byte zLSB = readRegister(L3G4200D_Address, 0x2C);
  z = ((zMSB << 8) | zLSB);
}

int setupL3G4200D(int scale){
  //From  Jim Lindblom of Sparkfun's code

  // Enable x, y, z and turn off power down:
  writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);

  // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
  writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);

  // Configure CTRL_REG3 to generate data ready interrupt on INT2
  // No interrupts used on INT1, if you'd like to configure INT1
  // or INT2 otherwise, consult the datasheet:
  writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);

  // CTRL_REG4 controls the full-scale range, among other things:

  if(scale == 250){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
  }else if(scale == 500){
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
  }else{
    writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
  }

  // CTRL_REG5 controls high-pass filtering of outputs, use it
  // if you'd like:
  writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
}

void writeRegister(int deviceAddress, byte address, byte val) {
    Wire.beginTransmission(deviceAddress); // start transmission to device 
    Wire.write(address);       // send register address
    Wire.write(val);         // send value to write
    Wire.endTransmission();     // end transmission
}

int readRegister(int deviceAddress, byte address){

    int v;
    Wire.beginTransmission(deviceAddress);
    Wire.write(address); // register to read
    Wire.endTransmission();

    Wire.requestFrom(deviceAddress, 1); // read a byte

    while(!Wire.available()) {
        // waiting
    }

    v = Wire.read();
    return v;
}
